extprod3d <-
function (x, y) 
{
    
x = matrix(x, ncol = 3)
y = matrix(y, ncol = 3)
drop(cbind(x[, 2] * y[, 3] - x[, 3] * y[, 2], x[, 3] * y[,1] - 
x[, 1] * y[, 3], x[, 1] * y[, 2] - x[, 2] * y[,1]))

} 

arrow3d <- function(p0=c(0,1,0),p1=c(1,1,1),s=0.05,theta=pi/4,n=3,...){ 
  ##    p0: start point 
  ##    p1: end point 
  ##     s: length of barb as fraction of line length 
  ## theta: opening angle of barbs 
  ##     n: number of barbs 
  ##   ...: args passed to lines3d for line styling 
  ## by Barry Rowlingson
  
  ## rotational angles of barbs 
  phi=seq(0,2*pi,len=n+1)[-1] 
  
  ## length of line 
  lp = sqrt(sum((p1-p0)^2)) 
  
  ## point down the line where the barb ends line up 
  cpt=(1-(s*lp*cos(theta)))*(p1-p0) 
  
  ## draw the main line 
  line = lines3d(c(p0[1],p1[1]),c(p0[2],p1[2]),c(p0[3],p1[3]),...) 
  
  ## need to find a right-angle to the line. So create a random point: 
  rpt = jitter(c( 
    runif(1,min(p0[1],p1[1]),max(p0[1],p1[1])), 
    runif(1,min(p0[2],p1[2]),max(p0[2],p1[2])), 
    runif(1,min(p0[3],p1[3]),max(p0[3],p1[3])) 
  )) 
  
  ## and if it's NOT on the line the cross-product gives us a vector at right angles: 
  r = extprod3d(p1-p0,rpt) 
  ## normalise it: 
  r = r / sqrt(sum(r^2)) 
  
  ## now compute the barb end points and draw: 
  pts = list() 
  for(i in 1:length(phi)){ 
    ptb=rotate3d(r,phi[i],(p1-p0)[1],(p1-p0)[2],(p1-p0)[3]) 
    lines3d( 
      c(p1[1],cpt[1]+p0[1]+lp*s*sin(theta)*ptb[1]), 
      c(p1[2],cpt[2]+p0[2]+lp*s*sin(theta)*ptb[2]), 
      c(p1[3],cpt[3]+p0[3]+lp*s*sin(theta)*ptb[3]), 
      ... 
    ) 
  } 
  return(line) 
}


cone3d <- function(base=c(0,0,0),tip=c(0,0,1),rad=1,n=30,draw.base=TRUE,qmesh=FALSE,
                   trans = par3d("userMatrix"), ...) {
  ax <- tip-base
  if (missing(trans) && !rgl.cur()) trans <- diag(4)
  ### is there a better way?
  if (ax[1]!=0) {
    p1 <- c(-ax[2]/ax[1],1,0)
    p1 <- p1/sqrt(sum(p1^2))
    if (p1[1]!=0) {
      p2 <- c(-p1[2]/p1[1],1,0)
      p2[3] <- -sum(p2*ax)
      p2 <- p2/sqrt(sum(p2^2))
    } else {
      p2 <- c(0,0,1)
    }
  } else if (ax[2]!=0) {
    p1 <- c(0,-ax[3]/ax[2],1)
    p1 <- p1/sqrt(sum(p1^2))
    if (p1[1]!=0) {
      p2 <- c(0,-p1[3]/p1[2],1)
      p2[3] <- -sum(p2*ax)
      p2 <- p2/sqrt(sum(p2^2))
    } else {
      p2 <- c(1,0,0)
    }
  } else {
    p1 <- c(0,1,0); p2 <- c(1,0,0)
  }
  degvec <- seq(0,2*pi,length=n+1)[-1]
  ecoord2 <- function(theta) {
    base+rad*(cos(theta)*p1+sin(theta)*p2)
  }
  i <- rbind(1:n,c(2:n,1),rep(n+1,n))
  v <- cbind(sapply(degvec,ecoord2),tip)
  if (qmesh) 
    ## minor kluge for quads -- draw tip twice
    i <- rbind(i,rep(n+1,n))
  if (draw.base) {
    v <- cbind(v,base)
    i.x <- rbind(c(2:n,1),1:n,rep(n+2,n))
    if (qmesh)  ## add base twice
      i.x <-  rbind(i.x,rep(n+2,n))
    i <- cbind(i,i.x)
  }
  if (qmesh) v <- rbind(v,rep(1,ncol(v))) ## homogeneous
  if (!qmesh)
    triangles3d(v[1,i],v[2,i],v[3,i],...)
  else
    return(rotate3d(qmesh3d(v,i,material=...), matrix=trans))
}     

UnitVector<-function(n){matrix(rep(1,n),n,1)}
sympower <- function(x,pow) {
  edecomp <- eigen(x)
  roots <- edecomp$val
  v <- edecomp$vec
  d <- roots^pow
  if(length(roots)==1) d <- matrix(d,1,1) else d <- diag(d)
  sympow <- v %*% d %*% t(v)
  sympow
}

P <-function(x) {
  y <- x %*% solve(t(x) %*% x) %*% t(x)
  y
}

Q <- function(x) {
  q <- diag(dim(x)[1]) - P(x)
  q
}


draw.arrow.3d <- function(p1,p0=c(0,0,0),...){
  arrow3d(p0,p1,s=0.05,n=10,...)
}
setup.3d <- function(a=6){
  require(geometry) 
  require(rgl) 
  open3d()
  decorate3d(xlim=c(-a,a),ylim=c(-a,a),zlim=c(-a,a),xlab="Person 1",
             ylab="Person 2",zlab="Person 3")
  grid3d("y+")
  grid3d("z+")
  grid3d("x+")
  spheres3d(0,0,0,radius=.1)
}      

make.plane <- function(x,y,color=rgb(0.95,.95,0.95)){
  X <- t(rbind(x,y))
  cQx <- t(Q(X) %*% rnorm(3))
  planes3d(cQx[1],cQx[2],cQx[3],color=color)
  return(c(cQx[1],cQx[2],cQx[3]))
}

project.y.onto.plane <- function(y,x1,x2,...) {
  X <- t(rbind(x1,x2))
  draw.arrow.3d(as.vector(P(X) %*% y),...)
}

draw.error <- function(x1,x2,y,...){
  X <- t(rbind(x1,x2))
  yhat <- P(X) %*% y
  draw.arrow.3d(y,as.vector(yhat),...)
}

project.y.onto.x <- function(y,x,...) {
  yhat <- as.vector(P(x) %*% y)
  draw.arrow.3d(yhat,...)
  return(yhat)
}


draw.one <- function(){draw.arrow.3d}(UnitVector(3))
draw.x1 <- function(){draw.arrow.3d(x1,color="blue")}
draw.x2 <- function(){draw.arrow.3d(x2,color="red")}
draw.y <- function(){draw.arrow.3d(y)}
draw.yhat <-function(){project.y.onto.plane(y,x1,x2,color="green")}
draw.e <- function(){draw.error(x1,x2,y,color="yellow")}
draw.sum <- function(x1,x2){draw.arrow.3d(x1+x2,color="orange")}
complete.parallelogram<- function(x1,x2){
  draw.arrow.3d(x1+x2,x1)
  draw.arrow.3d(x1+x2,x2)
}